/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#include "WrmnkTestController.h"

#include "Polynomial.h"

#include "Utils.h"

#include <iostream>

// Przestrzeń nazw
namespace RafMat
{
namespace SimEngine
{

using namespace std;
using namespace SimInterface;

const std::string WrmnkTestController::CONF_DA = "da";
const std::string WrmnkTestController::CONF_DB = "db";
const std::string WrmnkTestController::CONF_K = "k";
const std::string WrmnkTestController::CONF_LAMBDA = "lambda";

WrmnkTestController::WrmnkTestController(SimInterface::ISimulator &isim)
    : ControllerBase(isim, "WT", "WrmnkTestController"),
      m_dA(1), m_dB(1), m_k(1), m_lambda(0.95)
{
    reset();
}

double WrmnkTestController::simulateControllerStep(double setPoint, double processVar)
{
    // Dla uniknięcia ostrzeżenia kompilatora.
    (void) processVar;

    typedef vector<double> PolyType;

    // test WRMNK
    // Ponieważ pseudoregulator nie robi nic, setPoint można potraktować jako u(i) w
    // w układzie otwartym, zaś processVar jako y(i).
    //wrmnk->identification(processVar, setPoint, processVar);

    // Wyświetlenie
    //std::cout << "J: B = " << Polynomial(wrmnk->getBPoly()) << " A = " <<
    //             Polynomial(wrmnk->getAPoly()) << std::endl;

    mat_wrmnk->identify(setPoint, processVar);
    Polynomial A = mat_wrmnk->getEstimatedA();
    Polynomial B = mat_wrmnk->getEstimatedB();

    std::cout << "M: B = " << B << " A = " << A << std::endl;

    // Bypass
    return setPoint;
}

void WrmnkTestController::reset()
{
    wrmnk.reset(new Jano::WRMNK(m_k, m_dA, m_dB, m_lambda));
    mat_wrmnk.reset(new WrmnkARXWrapper(m_dA, m_dB, m_k, m_lambda));
}

//
// Sprawy konfiguracyjne
//

bool WrmnkTestController::setParameter(const std::string& param, const std::string& value) throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string tvalue = Utils::trimWhiteSpaces(value);

    // Strumień danych dla łańcucha argumentu.
    istringstream istr(tvalue);

    double tmpVal;
    istr >> tmpVal;

    if(istr.bad())
        throw iconf_error::syntaxError(*this, tvalue);

    if(tparam == CONF_DA)
    {
        m_dA = (unsigned)tmpVal;
        reset();
    }
    else if(tparam == CONF_DB)
    {
        m_dB = (unsigned)tmpVal;
        reset();
    }
    else if(tparam == CONF_K)
    {
        m_k = (unsigned)tmpVal;
        reset();
    }
    else if(tparam == CONF_LAMBDA)
    {
        m_lambda = tmpVal;
        reset();
    }
    else
        throw iconf_error::unrecognizedParam(*this, tparam);

    return true;
}

std::string WrmnkTestController::getParameter(const std::string& param) const throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string outStr;

    // Strumień danych dla łańcucha argumentu.
    ostringstream ostr(outStr);

    // Wiem, sieczka straszna:
    if(tparam == CONF_DA)
    {
        ostr << m_dA;
    }
    else if(tparam == CONF_DB)
    {
        ostr << m_dB;
    }
    else if(tparam == CONF_K)
    {
        ostr << m_k;
    }
    else if(tparam == CONF_LAMBDA)
    {
        ostr << m_lambda;
    }
    else
        return ControllerBase::getParameter(tparam);

    return ostr.str();
}

SimFile::ConfigWriter& WrmnkTestController::dumpSpecificPar(SimFile::ConfigWriter& o) const
{
    o.pushParam(CONF_DA, m_dA);
    o.pushParam(CONF_DB, m_dB);
    o.pushParam(CONF_K, m_k);
    o.pushParam(CONF_LAMBDA, m_lambda);
    return o;
}

}
}
